import math
import pandas as pd
import numpy as np
import matplotlib.pyplot as plt

df = pd.read_csv("database/record_general.csv")

# 读取原始数据
angle = df["angle"].to_list()
speed = df["speed"].to_list()


# 计算加速度（基于滤波后的速度）
acc = []
for i in range(1, len(speed) - 1, 1):
    acc.append((speed[i + 1] - speed[i - 1]) / 2 / 0.01)

# 绘制原始数据和滤波后的数据
ax0 = plt.subplot(1, 3, 1)
ax0.plot(angle, label="angle (filtered)")
ax0.plot(speed, label="speed (filtered)")
ax0.plot(acc, label="acc")
ax0.set_title("raw data")
ax0.set_xlabel("time")
ax0.set_ylabel("value")
ax0.legend()

# 散点图：角度与速度的关系
ax1 = plt.subplot(1, 3, 2)
ax1.scatter(angle, speed)
ax1.set_title("angle - speed")
ax1.set_xlabel("angle")
ax1.set_ylabel("speed")

# 散点图：加速度与角度的关系
ac0 = []
ag0 = []
for i in range(1, len(angle) - 2, 1):
    angle_pi = angle[i] / 180.0 * math.pi
    if not abs(acc[i]) < 0.01:
        ac0.append(acc[i])
        ag0.append(math.tan(angle_pi))

ax2 = plt.subplot(1, 3, 3)
ax2.scatter(ag0, ac0)

ax2.set_title("tan(angle) - acc")
ax2.set_xlabel("tan(angle)")
ax2.set_ylabel("acc")

# 最小二乘拟合
coefficients = np.polyfit(ag0, ac0, 1)
poly = np.poly1d(coefficients)
print("Linear Fit Coefficients:", coefficients)
fit_y = poly(ag0)
ax2.text(0.05, 0.9, f"y = {coefficients[0]:.2f}x + {coefficients[1]:.2f}")

# 绘制拟合线
ax2.plot(ag0, fit_y, color="red", label="Linear Fit")
ax2.legend()

plt.show()
